/*
 * @file peripheral_event.h
 * @author haibo
 * @date Dec 26, 2023
 * @brief Peripheral Event
 * Contact: haibo.yang@lgmgim.cn
 */
#pragma once
#include "agv_msgs/msg/agv_event.hpp"
#include "rclcpp/rclcpp.hpp"

class CEventManager {
public:
    CEventManager(): seq_(0) {}

    void init(std::shared_ptr<rclcpp::Node> node) {
        event_pub_ = node->create_publisher<agv_msgs::msg::AGVEvent>("/agvevent", 10);
        timer_ = node->create_wall_timer(std::chrono::seconds(1), 
                                        std::bind(&CEventManager::EventPublish, this));
    }

    void addEvent(const int ecode, int16_t event_react, int16_t typ, 
                 const std::string& message, int param1, int param2) {
        agv_msgs::msg::AGVEvent event;
        auto timestamp = std::chrono::system_clock::now();
        event.header.stamp = rclcpp::Time(std::chrono::duration_cast<std::chrono::nanoseconds>(
            timestamp.time_since_epoch()).count());
        event.header.frame_id = "battery_node";
            
        event.ecode = ecode;
        event.description = message;
        event.action = event_react;
        event.type = typ; 
        event.ackreq = false;
        event.param1 = param1;
        event.param2 = param2;
        event.eventtime = std::chrono::duration<double>(timestamp.time_since_epoch()).count();
        
        events_.push_back(event);
        publishEvent(event);
        RCLCPP_INFO_STREAM(rclcpp::get_logger("event_manager"), 
                          "[Peripheral] Add and publish event:" << event.description);
    }

    void updateEvent(const int ecode, int16_t event_react, int16_t typ, 
                    const std::string& message, int param1 = 0, int param2 = 0) {
        auto it = std::find_if(events_.begin(), events_.end(), 
                              [ecode](const agv_msgs::msg::AGVEvent& event) { 
                                  return event.ecode == ecode; 
                              });
    
        // 判断是否找到事件
        if (it != events_.end()) {
            auto timestamp = std::chrono::system_clock::now();
            it->header.stamp = rclcpp::Time(std::chrono::duration_cast<std::chrono::nanoseconds>(
                timestamp.time_since_epoch()).count());
            it->description = message;
            if (param1 != 0)
                it->param1 = param1;
            if (param2 != 0)
                it->param2 = param2;
                
            publishEvent(*it);
            RCLCPP_INFO_STREAM(rclcpp::get_logger("event_manager"), 
                              "[Peripheral] Update and publish event:" << it->description);
        } else {
            addEvent(ecode, event_react, typ, message, param1, param2);
        }
    }

    void clearEvent() {
        if (events_.size() > 0) {
            events_.clear();
        }
    }

    void removeEvent(const int ecode) {
        if (events_.size() > 0) {
            events_.erase(std::remove_if(events_.begin(), events_.end(), 
                [ecode](const agv_msgs::msg::AGVEvent& event) {
                    if (event.ecode == ecode) {
                        RCLCPP_INFO_STREAM(rclcpp::get_logger("event_manager"), 
                                          "[Peripheral] Delete event:" << event.description);
                        return true; 
                    } else {
                        return false; 
                    }
                }), events_.end());
        }
    }

    bool IsNomal(void) {
        return events_.size() == 0;
    }

    bool checkEvent(const int ecode) {
        auto it = std::find_if(events_.begin(), events_.end(), 
                              [ecode](const agv_msgs::msg::AGVEvent& event) { 
                                  return event.ecode == ecode; 
                              });

        return it != events_.end();
    }

    void publishEvent(const agv_msgs::msg::AGVEvent& event) {
        event_pub_->publish(event);
    }

    void publishNormal(void) {
        agv_msgs::msg::AGVEvent event;
        auto timestamp = std::chrono::system_clock::now();
        event.header.stamp = rclcpp::Time(std::chrono::duration_cast<std::chrono::nanoseconds>(
            timestamp.time_since_epoch()).count());
        event.header.frame_id = "battery_node";
            
        event.ecode = agv_msgs::msg::AGVEvent::NONE_EVENT;
        event.description = "No event";
        event.action = agv_msgs::msg::AGVEvent::NONE;
        event.type = agv_msgs::msg::AGVEvent::INFO;  
        event_pub_->publish(event);  
    }

    void EventPublish() {
        if (events_.size() == 0) {
            publishNormal();
        }
    }
    
private:
    rclcpp::Publisher<agv_msgs::msg::AGVEvent>::SharedPtr event_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::vector<agv_msgs::msg::AGVEvent> events_;
    unsigned int seq_;
};